/***************************************************************************
 创建者: 华磊
 开始时间: 2019.4.3
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 ***************************************************************************/
#include "positionregulatorgroup.h"
#include "positionregulator.h"
#include "domparser.h"
#include <QDebug>
#include "motionmessage.h"

#define D_PARAMETER_NODE_COUNT 8

PositionRegulatorGroup::PositionRegulatorGroup(QString filePathIn, double deltaTimeIn, MotionMessage *motionMessageIn,
                                               int robotIdIn, E_REGULATOR_TYPE typeIn)
{

    isInitialOk=false;
    motionMessage=motionMessageIn;
    deltaTime=deltaTimeIn;
    robotId=robotIdIn;
    regulatorType=typeIn;
    if(1!=readConfigFile(filePathIn))
    {
          addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22050, robotId);
    }
    else
    {
        for(int i=0;i<configInfos.size();i++)
        {
            PositionRegulator *tmpRegulator=new PositionRegulator(i,deltaTime,configInfos.value(i),typeIn);
            regulatorList.insert(i,tmpRegulator);

        }
        isInitialOk=true;
    }
}

int PositionRegulatorGroup::getPositionRegulatorDebugInfo(int regulatorIndex, RegulatorDebugInfo &infoOut)
{
//    usingMutex.lock();
    if(regulatorIndex<regulatorList.size() && regulatorIndex>=0)
    {
        regulatorList[regulatorIndex]->getPositionRegulatorDebugInfo(infoOut);
    }
//    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::setPositionInput(QVector<double> positionInput)
{
    usingMutex.lock();
    for(int i=0;i<positionInput.size();i++)
    {
        regulatorList[i]->setPositionInput(positionInput[i]);
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::setServoStatus(QVector<bool> isServoOn)
{
    usingMutex.lock();
    for(int i=0;i<regulatorList.size();i++)
    {
        if(i<isServoOn.size())
        {
            regulatorList[i]->setServoStatus(isServoOn[i]);
        }

    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::loopRegulatorOnce_withPosition(QVector<double> feedforwardPosition, QVector<double> targetPosition,
                                                           QVector<double> feedbackPosition,
                                                           QVector<double> &velOutut)
{
    if(false==isInitialOk)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22052, robotId);
        return -1;
    }

    usingMutex.lock();
    velOutut.resize(regulatorList.size());
    for(int i=0;i<regulatorList.size();i++)
    {
        if(i>=targetPosition.size() ||i>=feedbackPosition.size() )
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22051, robotId);
            usingMutex.unlock();
            return -1;
        }
        regulatorList[i]->setPositionInput(feedforwardPosition[i]);
        regulatorList[i]->loopRegulatorOnce(targetPosition[i],feedbackPosition[i],velOutut[i]);
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::loopRegulatorOnce(QVector<double> targetPosition, QVector<double> feedbackPosition, QVector<double> &velOutut)
{
    if(false==isInitialOk)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22052, robotId);
        return -1;
    }

    usingMutex.lock();
    velOutut.resize(regulatorList.size());
    for(int i=0;i<regulatorList.size();i++)
    {
        if(i>=targetPosition.size() ||i>=feedbackPosition.size() )
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22051, robotId);
            usingMutex.unlock();
            return -1;
        }
//        regulatorList[i]->setPositionInput(targetPosition[i]);error
        regulatorList[i]->loopRegulatorOnce(targetPosition[i],feedbackPosition[i],velOutut[i]);
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::loopRegulatorOnce( QVector<double> feedbackPosition, QVector<double> &velOutut)
{
    if(false==isInitialOk)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22052, robotId);
        return -1;
    }
    usingMutex.lock();
    velOutut.resize(regulatorList.size());
    for(int i=0;i<regulatorList.size();i++)
    {
        if(i>=feedbackPosition.size() )
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22051, robotId);
            usingMutex.unlock();
            return -1;
        }
        regulatorList[i]->loopRegulatorOnce(feedbackPosition[i],velOutut[i]);
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::isValid(QVector<bool> &isValidOut)
{
    if(false==isInitialOk)
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "PositionRegulatorGroup", 22052, robotId);
        return -1;
    }
    usingMutex.lock();
    isValidOut.resize(regulatorList.size());
    for(int i=0;i<regulatorList.size();i++)
    {
        isValidOut[i]=regulatorList[i]->isValid();
    }
    usingMutex.unlock();
    return 1;
}

bool PositionRegulatorGroup::isInitialOkStatus()
{
    return isInitialOk;
}

int PositionRegulatorGroup::setPositionPid(int axisIdIn, double pid_p, double pid_i, double pid_d,
                                           double errorLimitForIntegration, double feedforward)
{
    int returnKey;
    usingMutex.lock();
    if(true==regulatorList.contains(axisIdIn))
    {
        returnKey=regulatorList.value(axisIdIn)->setPositionPid(pid_p,pid_i,pid_d,errorLimitForIntegration,feedforward);
    }
    usingMutex.unlock();
    return returnKey;

}

int PositionRegulatorGroup::quickStop()
{
    usingMutex.lock();
    for(int i=0;i<regulatorList.size();i++)
    {
        regulatorList[i]->quickStop();
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::stop()
{
    usingMutex.lock();
    for(int i=0;i<regulatorList.size();i++)
    {
        regulatorList[i]->stop();
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::cease()
{
    usingMutex.lock();
    for(int i=0;i<regulatorList.size();i++)
    {
        regulatorList[i]->cease();
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::resetAll()
{
    usingMutex.lock();
    for(int i=0;i<regulatorList.size();i++)
    {
        regulatorList[i]->resetAll();
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::startRun()
{
    usingMutex.lock();
    for(int i=0;i<regulatorList.size();i++)
    {
        regulatorList[i]->startRun();
    }
    usingMutex.unlock();
    return 1;
}

int PositionRegulatorGroup::readConfigFile(QString filePathIn)
{
    configInfos.clear();

    DomParser parser;
    bool ok = parser.openXml(filePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开机器人数据文件！
    {
        return -1;
    }
    int robotAxisesCount=0;

    QDomNode  tmpNode;


    ///读参数
    QDomNode  inNode = parser.findSubNode(parser.getRootDomElement(), "robotAxis", ok );
    if( !ok )
    {
        qDebug()<<"PositionRegulatorGroup::readConfigFile===读文件失败: robotAxis"<<filePathIn;
        return -1;
    }

    parser.readXml( parser.findSubNode(inNode, "count", ok), robotAxisesCount  );
    if(!ok)
    {
        qDebug()<<"PositionRegulatorGroup::readConfigFile===读文件失败: count";
        return -1;
    }


    QVector<bool> okReturn;
    okReturn.resize(D_PARAMETER_NODE_COUNT);
    for(int i=0;i<robotAxisesCount;i++)
    {
         tmpNode =  parser.findSubNode(inNode, QString("axis%1").arg(i), ok);
         if( !ok  )
         {
            qDebug()<<"PositionRegulatorGroup::readConfigFile====读内部轴参数失败: axis "<<i;
            return -1;
         }

         RegulatorConfigInfo tmpInfo;

        //节点读取
        parser.readXml( parser.findSubNode(tmpNode, "propotial", okReturn[0]),  tmpInfo.propotial );
        parser.readXml( parser.findSubNode(tmpNode, "integration", okReturn[1]),  tmpInfo.integration );
        parser.readXml( parser.findSubNode(tmpNode, "differential", okReturn[2]),  tmpInfo.differential);
        parser.readXml( parser.findSubNode(tmpNode, "minOutput", okReturn[3]),  tmpInfo.minOutput );
        parser.readXml( parser.findSubNode(tmpNode, "maxOutput", okReturn[4]),  tmpInfo.maxOutput);
        parser.readXml( parser.findSubNode(tmpNode, "maxOutput", okReturn[5]),  tmpInfo.feedforward);
        parser.readXml( parser.findSubNode(tmpNode, "isValid", okReturn[6]),  tmpInfo.isValid);
        parser.readXml( parser.findSubNode(tmpNode, "errorLimitForIntegration", okReturn[7]),  tmpInfo.errorLimitForIntegration);

        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              qDebug()<<"PositionRegulatorGroup::readConfigFile====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              return -1;
           }
        }
        configInfos.insert(i,tmpInfo);
    }


     parser.closeXml( );

    return 1;
}



void PositionRegulatorGroup::addMsg(int messageLevel, std::string componentName, std::string messageType
                            , int messageCode, int _robotId, int parameter1, int parameter2)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = _robotId;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 22052://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")PositionRegulatorGroup位置调节器初始化失败！").toStdString();
        infomationStr = str1;
        break;
    }

    case 22051://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")PositionRegulatorGroup轴数配置错误！").toStdString();
        infomationStr = str1;
        break;
    }

    case 22050://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")regulator配置文件解析失败！").toStdString();
        infomationStr = str1;
        break;
    }



        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}

